﻿/*=========================================================================
 * AUTH: Vincent Breton
 * DESC: Path Following Steering Behavior
 *=========================================================================*/
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework.Graphics;   //   for Texture2D
using Microsoft.Xna.Framework;  //  for Vector2
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Media;
using Microsoft.Xna.Framework.Net;
using Microsoft.Xna.Framework.Storage;

namespace RisingThreat
{
    class PathFollowing : Seek
    {
        public LevelGraph graph;
        public Stack<LevelGraphEdge> path;
        public LevelGraphNode startNode;
        public LevelGraphNode goalNode;
        public Vector3 startPosition;
        public Vector3 goalPosition;
        public int currentIndex;


        public PathFollowing()
        {

        }

        public PathFollowing(LevelGraph g)
        {
            graph = g;
            currentIndex = -1;
        }

        public void findStartNode()
        {
           //startNode = graph.findNearestNode(startPosition);
            startNode = findClosestNode(startPosition);
        }

        public void findGoalNode()
        {

            //goalNode = graph.findNearestNode(goalPosition);
            goalNode = findClosestNode(goalPosition);
        }

        public LevelGraphNode findClosestNode(Vector3 position)
        {
            float distance = 0;
            LevelGraphNode node = new LevelGraphNode(0, 0, '0');
            float shortestDistance = float.MaxValue;
            for (int i = 0; i < graph.nodes.Count; i++)
            {
                distance = (graph.nodes[i].position - position).Length();
                if (distance < shortestDistance)
                {
                    shortestDistance = distance;
                    node = graph.nodes[i];
                }
            }
            
            return node;

        }

        public void findPath()
        {
            findStartNode();
            findGoalNode();
            path = graph.aStar(startNode, goalNode);
        }

        public void printPath()
        {
            while(path.Count >= 0)
            {
                LevelGraphEdge edge = path.Pop();
            }
        }

        public void followPath()
        {
            if (path != null)
            {
                float distance = (goalPosition - steer.position).Length();
                if (distance < 1.0f) //arrived at goal
                {
                    seekHelper();
                    currentIndex = -1;
                    return;
                }
                if (currentIndex == -1) //start of path
                {
                    if (path.Count > 0)
                    {
                        currentIndex = 0;
                        LevelGraphEdge edge = path.Pop();
                        target = edge.fromNode.position;
                    }
                }

                distance = (target - steer.position).Length();

                if (distance < 1.0f) //arrived at target
                {
                    currentIndex++;
                    if (path.Count == 0)//arrived at Goal move toward goal position
                    {
                        target = goalPosition;
                    }
                    else
                    {
                        LevelGraphEdge edge = path.Pop();
                        target = edge.fromNode.position;
                    }
                }

                seekHelper();
            }
            return;
        }

        public Steering getSteering(Steering s)
        {
            steer = s;
            followPath();
            return steer;
        }
    }
}
